// Copyright 2022 Chen Jun

#ifndef ARMOR_PROCESSOR_TRACKER_HPP_
#define ARMOR_PROCESSOR_TRACKER_HPP_

// Eigen
#include "hnurm_armor/DataType.hpp"
#include "hnurm_armor/armor.hpp"
#include "hnurm_armor/extended_kalman_filter.hpp"
#include <Eigen/Eigen>

#include <rclcpp/rclcpp.hpp>
#include <vector>

namespace hnurm
{
class Tracker
{
public:
    // explicit Tracker(const cv::FileNode& cfg_node);
    explicit Tracker(rclcpp::Node::SharedPtr node);

    Tracker(double max_match_distance, int tracking_threshold, int lost_threshold);

    void init(const std::vector<Armor> &armors_msg);

    void update(const std::vector<Armor> &armors_msg);

    enum State
    {
        LOST,
        DETECTING,
        TRACKING,
        TEMP_LOST,
        JUMP,
    } tracker_state;

    ExtendedKalmanFilter ekf;

    ArmorsNum tracked_armors_num;

    Armor           tracked_armor;
    std::string     tracked_id;
    Eigen::VectorXd target_state;
    Eigen::VectorXd measurement;

    double info_position_diff = 0;
    double info_yaw_diff = 0;

    // To store another pair of armors message
    double     dz {}, another_r {};
    static int i_jump;
    double armor_pitch{};
    double armor_yaw{};
    // todo
    double orientationToYaw(const Eigen::Matrix3d &R);

private:
    void initEKF(const Armor &a);

    void handleArmorJump(const Armor &a);

    static Eigen::Vector3d getArmorPositionFromState(const Eigen::VectorXd &x);

    void updateArmorsNum(const Armor &armor);

    double max_match_distance_ {};
    double max_match_yaw_diff_ {};

    int tracking_threshold_ {};
    int lost_threshold_ {};

    int detect_count_ {};
    int lost_count_ {};

    double last_yaw_ {};

};

}  // namespace hnurm

#endif  // ARMOR_PROCESSOR_TRACKER_HPP_
